(in-package #:org.shirakumo.fraf.trial)

(declaim (inline /*))
(defun /* (a b)
  (if (<= (abs b) 0.0000001f0)
      0.0
      (/ a b)))

(declaim (inline mcol3))
(defun mcol3 (mat n &optional (vec (vec3 0 0 0)))
  (declare (type mat4 mat))
  (with-fast-matref (e mat)
    (setf (vx3 vec) (e 0 n)
          (vy3 vec) (e 1 n)
          (vz3 vec) (e 2 n))
    vec))

(declaim (ftype (function (vec3 (or mat4 mat3)) vec3) ntransform-inverse))
(defun ntransform-inverse (vec3 mat)
  (declare (optimize speed))
  (declare (type vec3 vec3))
  (declare (dynamic-extent mat))
  (etypecase mat
    (mat4
     (with-fast-matref (m mat)
       (let ((x (- (vx3 vec3) (m 0 3)))
             (y (- (vy3 vec3) (m 1 3)))
             (z (- (vz3 vec3) (m 2 3))))
         (setf (vx3 vec3) (+ (* x (m 0 0)) (* y (m 1 0)) (* z (m 2 0))))
         (setf (vy3 vec3) (+ (* x (m 0 1)) (* y (m 1 1)) (* z (m 2 1))))
         (setf (vz3 vec3) (+ (* x (m 0 2)) (* y (m 1 2)) (* z (m 2 2)))))))
    (mat3
     (with-fast-matref (m mat)
       (let ((x (vx3 vec3))
             (y (vy3 vec3))
             (z (vz3 vec3)))
         (setf (vx3 vec3) (+ (* x (m 0 0)) (* y (m 1 0)) (* z (m 2 0))))
         (setf (vy3 vec3) (+ (* x (m 0 1)) (* y (m 1 1)) (* z (m 2 1))))
         (setf (vz3 vec3) (+ (* x (m 0 2)) (* y (m 1 2)) (* z (m 2 2))))))))
  vec3)

(defun compute-world-inertia-tensor (iitworld iitbody rotmat)
  (let ((iitworld (marr3 iitworld))
        (iitbody (marr3 iitbody))
        (rotmat (marr4 rotmat)))
    (declare (optimize speed (safety 0)))
    (let ((t4  (+ (* (aref rotmat 0) (aref iitbody 0))
                  (* (aref rotmat 1) (aref iitbody 3))
                  (* (aref rotmat 2) (aref iitbody 6))))
          (t9  (+ (* (aref rotmat 0) (aref iitbody 1))
                  (* (aref rotmat 1) (aref iitbody 4))
                  (* (aref rotmat 2) (aref iitbody 7))))
          (t14 (+ (* (aref rotmat 0) (aref iitbody 2))
                  (* (aref rotmat 1) (aref iitbody 5))
                  (* (aref rotmat 2) (aref iitbody 8))))
          (t28 (+ (* (aref rotmat 4) (aref iitbody 0))
                  (* (aref rotmat 5) (aref iitbody 3))
                  (* (aref rotmat 6) (aref iitbody 6))))
          (t33 (+ (* (aref rotmat 4) (aref iitbody 1))
                  (* (aref rotmat 5) (aref iitbody 4))
                  (* (aref rotmat 6) (aref iitbody 7))))
          (t38 (+ (* (aref rotmat 4) (aref iitbody 2))
                  (* (aref rotmat 5) (aref iitbody 5))
                  (* (aref rotmat 6) (aref iitbody 8))))
          (t52 (+ (* (aref rotmat 8) (aref iitbody 0))
                  (* (aref rotmat 9) (aref iitbody 3))
                  (* (aref rotmat 10) (aref iitbody 6))))
          (t57 (+ (* (aref rotmat 8) (aref iitbody 1))
                  (* (aref rotmat 9) (aref iitbody 4))
                  (* (aref rotmat 10) (aref iitbody 7))))
          (t62 (+ (* (aref rotmat 8) (aref iitbody 2))
                  (* (aref rotmat 9) (aref iitbody 5))
                  (* (aref rotmat 10) (aref iitbody 8)))))
      (setf (aref iitworld 0) (+ (* t4  (aref rotmat 0))
                                 (* t9  (aref rotmat 1))
                                 (* t14 (aref rotmat 2)))
            (aref iitworld 1) (+ (* t4  (aref rotmat 4))
                                 (* t9  (aref rotmat 5))
                                 (* t14 (aref rotmat 6)))
            (aref iitworld 2) (+ (* t4  (aref rotmat 8))
                                 (* t9  (aref rotmat 9))
                                 (* t14 (aref rotmat 10)))
            (aref iitworld 3) (+ (* t28 (aref rotmat 0))
                                 (* t33 (aref rotmat 1))
                                 (* t38 (aref rotmat 2)))
            (aref iitworld 4) (+ (* t28 (aref rotmat 4))
                                 (* t33 (aref rotmat 5))
                                 (* t38 (aref rotmat 6)))
            (aref iitworld 5) (+ (* t28 (aref rotmat 8))
                                 (* t33 (aref rotmat 9))
                                 (* t38 (aref rotmat 10)))
            (aref iitworld 6) (+ (* t52 (aref rotmat 0))
                                 (* t57 (aref rotmat 1))
                                 (* t62 (aref rotmat 2)))
            (aref iitworld 7) (+ (* t52 (aref rotmat 4))
                                 (* t57 (aref rotmat 5))
                                 (* t62 (aref rotmat 6)))
            (aref iitworld 8) (+ (* t52 (aref rotmat 8))
                                 (* t57 (aref rotmat 9))
                                 (* t62 (aref rotmat 10))))))
  iitworld)
